#pragma once
#include <string>
#include <chrono>
#include <cmath>
#include "AiBox_LOG.h"
//#define M_PI      (3.141592653589793f)
#define DEG_TO_RAD      (M_PI / 180.0f)
#define RAD_TO_DEG      (180.0f / M_PI)
#define LOCATION_SCALING_FACTOR_M     0.011131884502145034f
#define LOCATION_SCALING_FACTOR_INV   89.83204953368922f
/*must set the origin point*/
namespace Planner
{
class vectorNE
{
    public:
        vectorNE(){};
        vectorNE(const vectorNE& temp){_n=temp._n;_e=temp._e;}
        vectorNE(double N_m,double E_m):_n(N_m),_e(E_m){}
        vectorNE& operator=(const vectorNE& temp){_n=temp._n;_e=temp._e;return *this;}
        double _n;  //m
        double _e;  //m
};
class LocationInfo
{
    public:
        LocationInfo(int32_t lat,int32_t lon,int32_t alt=0);
        LocationInfo(double N_m,double E_m,int32_t alt=0)
        :_vector_ne(N_m,E_m)
        ,_alt(alt)
        ,_timestamp(CURRENT_TIME)
        {
            const int32_t dlat =  _vector_ne._n * LOCATION_SCALING_FACTOR_INV;
            const int32_t dlng = (_vector_ne._e * LOCATION_SCALING_FACTOR_INV) / longitudeScale();
            _lat = _origin_lat+dlat;
            _lon = _origin_lon+dlng;
        }
        LocationInfo(const LocationInfo& temp)
        :_lat(temp._lat)
        ,_lon(temp._lon)
        ,_alt(temp._alt)
        ,_vector_ne(temp._vector_ne)
        ,_timestamp(temp._timestamp)
        {
        }
        LocationInfo& operator=(const LocationInfo& temp)
        {
            this->_lat=temp._lat;
            this->_lon=temp._lon;
            this->_alt=temp._alt;
            this->_vector_ne=temp._vector_ne;
            this->_timestamp=temp._timestamp;
            return *this;
        }
        double distanceTo(const LocationInfo& tempinfo)const;
        double normal();
        float longitudeScale()
        {
            float scale = cosf(_lat * (1.0e-7f * DEG_TO_RAD));
            return fmax(scale, 0.01f);
        }
        const vectorNE getNEVector() const {return _vector_ne;}
        int32_t getLat(){return _lat;}
        int32_t getLon(){return _lon;}
        int32_t getAlt(){return _alt;}
        static int32_t getOriginLat(){return _origin_lat;}
        static int32_t getOriginLon(){return _origin_lon;}
        static int32_t getOriginAlt(){return _origin_alt;}
        static void setOriginLat(int32_t lat){_origin_lat=lat;}
        static void setOriginLon(int32_t lon){_origin_lon=lon;}
        static void setOriginAlt(int32_t alt){_origin_alt=alt;}
        std::chrono::milliseconds getTimestamp(){return _timestamp;}
    private:
        int32_t _lat=0; //deg*10E7
        int32_t _lon=0; //deg*10e7
        int32_t _alt=0; //m*1000
        vectorNE _vector_ne;
        std::chrono::milliseconds _timestamp; //ms
        static int32_t _origin_lat;
        static int32_t _origin_lon;
        static int32_t _origin_alt;
};
};